//includes for ROS and messages
#include "ros/ros.h"
#include <std_msgs/Int32.h>
#include <std_msgs/Int8MultiArray.h>

//includes for cvopen
#include "cv.h"
#include "highgui.h"

//include something I dont know ;)
#include <iostream>
#include <string.h>
#include <signal.h>

#define IMAGEWIDTH 160
#define IMAGEHEIGHT 120

void identify_tag(IplImage *img)
{
  //Hej guys :) I think first of all an extra node is way easier to test and then we can decide later where we exactly put the method. 
  // For Tests you just have to run the CameraNode on the Robot and then this one (better on computer first :) ) 
  //This node just receives an image and then gives it to this method :)
  //Its already in CMakeList
}

void cam0_cb(const std_msgs::Int8MultiArray::ConstPtr& array)
{
	printf("cam0_cb\n");
	IplImage *img 				= cvCreateImage(cvSize(IMAGEWIDTH, IMAGEHEIGHT), IPL_DEPTH_8U, 3);
	char * data 				= img->imageData;
	
	for(int i = 0; i < IMAGEWIDTH*IMAGEHEIGHT*3; i++)
	{
		data[i] = char(array->data.at(i));
	}
	identify_tag(img);
	ros::spinOnce();
  cvReleaseImage(&img);
}

void cam1_cb(const std_msgs::Int8MultiArray::ConstPtr& array)
{
	printf("cam1_cb\n");
	
	IplImage *img 				= cvCreateImage(cvSize(IMAGEWIDTH, IMAGEHEIGHT), IPL_DEPTH_8U, 3);
	char * data 				= img->imageData;
	
	for(int i = 0; i < IMAGEWIDTH*IMAGEHEIGHT*3; i++)
	{
		data[i] = char(array->data.at(i));
	}
	
  ros::spinOnce();
  cvReleaseImage(&img);
}

int main(int argc, char **argv)
{
	printf("Starting TagIdentification\n");
	ros::init(argc, argv, "TagIdentificator");
	ros::NodeHandle n;
	ros::Subscriber img0_sub = n.subscribe("/camera0_img", 1, cam0_cb);
	ros::Subscriber img1_sub = n.subscribe("/camera1_img", 1, cam1_cb);
	ros::spin();
}
